/**
 * @file:          Filter_BinarySecondOrder.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.07.28
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.07.28,16:51:29
 */

/* Include Files **************************************************************/
#include "Filter_BinarySecondOrder.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
void Filter_BinarySecondOrder_Init(struct Filter_BinarySecondOrder *self)
{
    self->Run = Filter_BinarySecondOrder_Run;
    self->Clear = Filter_BinarySecondOrder_Clear;
    self->UpdateParam = Filter_BinarySecondOrder_UpdateParam;

    self->UpdateParam(self);
}

float32 Filter_BinarySecondOrder_Run(struct Filter_BinarySecondOrder *self, float32 in)
{
    if (self->param_en != 0)
    {
        self->y_last2 = self->y_last1;
        self->y_last1 = self->out_y;
        self->x_last2 = self->x_last1;
        self->x_last1 = self->in_x;
        self->in_x = in;

        self->out_y = self->m[0] * self->in_x +
                      self->m[1] * self->x_last1 +
                      self->m[2] * self->x_last2 +
                      self->n[0] * self->y_last1 +
                      self->n[1] * self->y_last2;
    }
    else
    {
        self->in_x = in;
        self->out_y = self->in_x;
    }

    return self->out_y;
}

void Filter_BinarySecondOrder_Clear(struct Filter_BinarySecondOrder *self)
{
    self->in_x = 0;
    self->out_y = 0;
    self->x_last1 = 0;
    self->x_last2 = 0;
    self->y_last1 = 0;
    self->y_last2 = 0;
}

void Filter_BinarySecondOrder_UpdateParam(struct Filter_BinarySecondOrder *self)
{
    float32 a;
    float32 b;
    a = squre(BM_PI * self->param_Ts * self->param_fc);
    b = BM_PI * self->param_Ts * self->param_fc;
    self->m[0] = (a - 2 * BM_PI * self->param_Ts * self->param_depth * self->param_fc * self->param_zeta_0N * sqrt(1 / self->param_depth) + self->param_depth) / (a + 2 * b * self->param_zeta_0D + 1);
    self->m[1] = (2 * a - 2 * self->param_depth) / (a + 2 * b * self->param_zeta_0D + 1);
    self->m[2] = (a + 2 * BM_PI * self->param_Ts * self->param_depth * self->param_fc * self->param_zeta_0N * sqrt(1 / self->param_depth) + self->param_depth) / (a + 2 * b * self->param_zeta_0D + 1);
    self->n[0] = (-2 * a + 2) / (a + 2 * b * self->param_zeta_0D + 1);
    self->n[1] = (-a + 2 * b * self->param_zeta_0D - 1) / (a + 2 * b * self->param_zeta_0D + 1);
}